/*
 * Copyright 2021 <copyright holder> <email>
 * 
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * 
 *     http://www.apache.org/licenses/LICENSE-2.0
 * 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <iostream>
#include <fstream>
#include <string>

#include <g2o/types/slam3d/types_slam3d.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>

using namespace std;

int main(int argc,char **argv){
    if(argc!=2){
        cout<<"Usage: pose_graph_g2o_SE3 sphere.g2o"<<endl;
        return 1;
    }
    ifstream fin(argv[1]);
    if(!fin){
        cout<<"file "<<argv[1]<<"does not exist."<<endl;
        return 1;
    }
    
    //设定g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,6>> BlockSolverType;
    typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType> LinearSolverType;
    auto solvers=new g2o::OptimizationAlgorithmLevenberg(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    g2o::SpareOptimizer optimizer; //图模型
    optimizer.setAlgorithm(solver); //设置求解器
    optimizer.setVerbose(true);     //打开调试输出
    
    int vertexCnt=0,edgeCnt=0; //顶点和边的个数
    while(!fin.eof()){
        string name;
        fin>>name;
        if(name=="VERTEX_SE3:;QUAT"){
            //se3顶点
            g2o::VertexSE3*v =new g2o::VertexSE3();
            int index=0;
            v->setId(index);
            v->read(fin);
            optimizer.addVertex(v);
            vertexCnt++;
            if(index==0)
                v->setFixed(true);
        }else if(name=="EDGE_SE3:QUAT"){
            //SE3-SE3边
            g2o::EdgeSE3 *e=new g2o::EdgeSE3();
            int idx1,idx2; //关联的两个顶点
            fin>>idx1,idx2;
            e->setId(edgeCnt++);
            e->setVertex(0,optimizer.vertices()[idx1]);
            e->setVertex(1,optimizer.vertices()[idx2]);
            e->read(fin);
            optimizer.addEdge(e);
        }
        if(!fin.good()) break;
    }
    
    cout<<"read total "<<vertexCnt<<" vertices, "<<edgeCnt<<" edges. "<,endl;
    
    cout<<"optimizing ..."<<endl;
    optimizer.initializeOptimization();
    optimizer.optimize(30);
    cout<<"saving optimization results ... "<<endl;
    optimizer.save("result.g2o");
    
    return 0;
}
    
            
            
            
            
            
            
            
            
            
            
            
            
            
            
            
    
